
#include "sdfs_tcp_pkg/sriCommManager.h"

CSRICommManager::CSRICommManager()
{
}

CSRICommManager::~CSRICommManager()
{
}

// 初始化
bool CSRICommManager::Init()
{  
	mTCPClient.OpenTCP("192.168.0.108", 4008, "192.168.154.128", 4008);
	mTCPClient.AddCommParser(&mATParser);
	// mTCPClient.AddCommParser(&mM8218Parser);
	//bind Network communication failure processing function
	//绑定网络通讯失败处理函数
	SRICommNetworkFailureCallbackFunction networkFailureCallback = std::bind(&CSRICommManager::OnNetworkFailure, this, std::placeholders::_1);
	mTCPClient.SetNetworkFailureCallbackFunction(networkFailureCallback);

	// //bind ACK command Processing function
	// 绑定ACK指令处理函数
	SRICommATCallbackFunction atCallback = std::bind(&CSRICommManager::OnCommACK, this, std::placeholders::_1);
	mATParser.SetATCallbackFunction(atCallback);

	// //bind M8128 Data processing function
	//绑定M8128数据处理函数
	// SRICommM8218CallbackFunction m8218Callback = std::bind(&CSRICommManager::OnCommM8218, this,
	// 	std::placeholders::_1, std::placeholders::_2, std::placeholders::_3,
	// 	std::placeholders::_4, std::placeholders::_5, std::placeholders::_6);
	// mM8218Parser.SetM8218CallbackFunction(m8218Callback);
	if (mTCPClient.Connect() == false)
	{
		return false;
	}
	if (SendCommand("SMPF", "200") == false)
	{
		return false;
	}

	// Send command to M8128 to set data check mode
	// 向m8128发送指令，设置数据校验模式
	if (SendCommand("DCKMD", "SUM") == false)
	{
		return false;
	}
	switch01 = 1;
	if (SendCommand("GSD", "") == false)
	{
		// GSD has no ACK
		return false;
	}
	return true;
}
// 启动
bool CSRICommManager::Run()
{
	// creat a TCP client connection
	// 创建TCP连接

	// Send command to M8128 to set sampling rate
	//
	// 向m8128发送指令，设置采样率

	// Send command to M8128 Start to upload sensor data continuously。
	// 向m8128发送指令，开始连续上传传感器数据

	if (SendCommand("GOD", "") == false)
	{
		// GSD has no ACK
		return false;
	}
	////Send command to M8128 to set data upload format
	////向m8128发送指令，设置数据上传格式
	// if (SendCommand("SGDM", "(A01,A02,A03,A04,A05,A06);E;1;(WMA:1)") == false)
	//{
	//	return false;
	// }
	return true;
}

bool CSRICommManager::Stop()
{
	mTCPClient.CloseTCP();
	return true;
}

// 通讯异常函数
bool CSRICommManager::OnNetworkFailure(std::string infor)
{
	printf("OnNetworkFailure = %s\n", infor.data());
	return true;
}

// send command
// 发送命令
bool CSRICommManager::SendCommand(std::string command, std::string parames)
{
	mIsGetACK = false;
	mCommandACK = "";
	mParamesACK = "";

	// Combination command
	// 组合AT指令
	std::string atCommand = "AT+" + command + "=" + parames + "\r\n";
	if (mTCPClient.OnSendData((BYTE *)atCommand.data(), (int)atCommand.length()) == false)
	{
		return false;
	}
	// wait ACK

	timeval start, end;
	gettimeofday(&start, NULL);
	while (true)
	{
		if (mIsGetACK == true)
		{
			break;
		}
		gettimeofday(&end, NULL);
		long span = 1000 * (end.tv_sec - start.tv_sec) + (end.tv_usec - start.tv_usec) / 1000;
		// if (span >= 10000) // 10s
		// {
		// 	return false;
		// }
	}

	if (mCommandACK != command)
	{
		// ACK command error
		return false;
	}
	//
	printf("ACK+%s=%s", mCommandACK.data(), mParamesACK.data());
	//
	return true;
}

// ACK command processing
// ACK应答处理
bool CSRICommManager::OnCommACK(std::string command)
{
	int index = (int)command.find('=');
	if (index == -1)
	{
		mCommandACK = command;
		mParamesACK = "";
	}
	else
	{
		mCommandACK = command.substr(0, index);
		mParamesACK = command.substr(index + 1);
	}
	mCommandACK = mCommandACK.substr(4);

	mIsGetACK = true; // 应答正确

	return true;
}
// M8128 data
// M8128数据
// bool CSRICommManager::OnCommM8218(float fx, float fy, float fz, float mx, float my, float mz)
// {
// 	forcex = fx;
// 	forcey = fy;
// 	forcez = fz;
// 	torquex = mx;
// 	torquey = my;
// 	torquez = mz;
// 	printf("M8218 = %f, %f, %f,   %f, %f, %f\n", fx, fy, fz, mx, my, mz);
// 	return true;
// }